require("socket")
require("max")

local robot = max.connect()
if (robot == nil) then
	print("Error: could not connect to robot")
	os.exit(1)
end

local port = 10301
if (host == nil) then
	host = "*"
end

print("Starting UDP receiver...")
local udp = socket.udp()

print("Listening for client '" .. host .. "' on port " .. port)
assert(udp:setsockname("*", port))
assert(udp:setpeername(host, 0))
udp:settimeout(0.25)

function sleep(n)
	os.execute("sleep " .. tonumber(n))
end

print("Waiting for packets...");
local pitr = 0		--packet iterator, will only accept commands that have this number (opcode 0 changes it)
while (1) do
	repeat
		local dgram = udp:receive()
	
		if (dgram) then
	
			local opcode = tonumber(string.sub(dgram, 1, 1))
			local lpitr = tonumber(string.sub(dgram, 2, 4))		--local packet iterator
		
			if (opcode == nil or lpitr == nil) then
				break
			end
		
			if (opcode == 0) then
				--im alive command
				pitr = lpitr
			elseif (pitr ~= lpitr) then
				--lpitr is invalid
				io.write("[P]")
			else
				if (opcode == 1) then
					local value = string.sub(dgram, 5)
					if (value == nil) then
						break
					end
					
					local name = {string.find(value, "(%a+):")}
					if (name[3] == "videoparams") then
						local p = {string.find(value, "videoparams:(%d+),(%d+),(%d+)")}
						local w = tonumber(p[3])
						local h = tonumber(p[4])
						local fps = tonumber(p[5])
						
						if (w == nil or h == nil or fps == nil) then
							break
						end
						
						robot.videoparams(w, h, fps)
						
					else
						io.write("[Etxt]")
					end
					
				
				elseif (opcode > 1 and opcode <= 7) then
					--packet is a numbered one
					local value = tonumber(string.sub(dgram, 5))
					assert(value)
				
					if (opcode == 2) then
						robot.motor(value)
					--elseif (opcode == 1) then
					--	robot.direction_set(value)
					elseif (opcode == 3) then
						robot.turn(value)
					elseif (opcode == 4) then
						robot.turnfront(value)
					elseif (opcode == 5) then
						robot.turnrear(value)
					elseif (opcode == 6) then
						robot.pan(value)
					elseif (opcode == 7) then
						robot.tilt(value);
					end
				else
					--unknown packet
					io.write("Enum")
				end
			end
		else
			robot.motor(0)
			pitr = 0
			io.write(".")
			sleep(1)
		end
		io.flush()
	until true
end
